#include <FU68xx_2.h>
#include <Myproject.h>

PLLVar         data PLLfunction;

void PLLSoftwareFunction1(void)
{
    LPF1_X = FOC__VALP;
    LPF1_YH = PLLfunction.ualp;
    LPF1_YL = PLLfunction.ualp_LSB;
    SMDU_RunBlock(1, LPF);
    PLLfunction.ualp = LPF1_YH;
    PLLfunction.ualp_LSB = LPF1_YL;
    
    MUL3_MA = PLLfunction.ealp;
    SMDU_RunBlock(3,S1MUL);    
    PLLfunction.e_alp_delta = (MUL3_MCH << 2);
    
    MUL2_MA = PLLfunction.ualp;    
    SMDU_RunBlock(2,S1MUL);    
    PLLfunction.u_part = (MUL2_MCH << 1);
    
    PLLfunction.ialp_pre = PLLfunction.ialp + PLLfunction.u_part - PLLfunction.e_alp_delta;
    
    LPF1_X = FOC__IA;
    LPF1_YH = PLLfunction.ialp;
    LPF1_YL = PLLfunction.ialp_LSB;
    SMDU_RunBlock(1, LPF);
    PLLfunction.ialp = LPF1_YH;
    PLLfunction.ialp_LSB = LPF1_YL;
    
    PI2_EK =  PLLfunction.ialp_pre - PLLfunction.ialp;                
    SMDU_RunBlock(2, PI);
    PLLfunction.ealp = PI2_UKH;

    
    LPF1_X = FOC__VBET;
    LPF1_YH = PLLfunction.ubet;
    LPF1_YL = PLLfunction.ubet_LSB;
    SMDU_RunBlock(1, LPF);
    PLLfunction.ubet = LPF1_YH;
    PLLfunction.ubet_LSB = LPF1_YL;
    
    MUL3_MA = PLLfunction.ebet;
    SMDU_RunBlock(3,S1MUL);
    PLLfunction.e_bet_delta = (MUL3_MCH << 2);
    
    MUL2_MA = PLLfunction.ubet;
    SMDU_RunBlock(2,S1MUL);
    PLLfunction.u_part = (MUL2_MCH << 1);
    PLLfunction.ibet_pre = PLLfunction.ibet + PLLfunction.u_part - PLLfunction.e_bet_delta;
    LPF2_X = FOC__IBET;
    SMDU_RunBlock(2, LPF);
    PLLfunction.ibet = LPF2_YH;
}


void PLLSoftwareFunction2(int16 angle_com)
{
    PI3_EK =  PLLfunction.ibet_pre - PLLfunction.ibet;                
    SMDU_RunBlock(3, PI);
    PLLfunction.ebet = PI3_UKH; 
    SCAT0_SIN = -PLLfunction.ealp;
    SCAT0_COS = PLLfunction.ebet;
    SMDU_RunBlock(0,ATAN);
    PLLfunction.PLLTheta = SCAT0_RES2 + angle_com;

    PLLfunction.Theta_err = PLLfunction.PLLTheta - PLLfunction.Theta_pre;   

    if ((PLLfunction.Theta_err > 320) && (PLLfunction.count > 100))
    {
        FOC__THETA = PLLfunction.PLLTheta;
        PLLfunction.Theta_pre = PLLfunction.PLLTheta;

    }
    else
    {
        FOC__THETA = PLLfunction.test;
        PLLfunction.Theta_pre = PLLfunction.test;

    }
     
    LPF3_X = PLLfunction.PLLTheta - PLLfunction.PLLTheta_pre;
    SMDU_RunBlock(3, LPF);
    PLLfunction.PLLTheta_pre = PLLfunction.PLLTheta; 
    PLLfunction.Speed_PLL = LPF3_YH;
    
}
